
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sitl.c
  * @author     baiyang
  * @date       2022-11-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sitl.h"
#include "sim_gps.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/
#define BARO_GROUPINFO(x) PARAM_DEFINE_FLOAT(x##RND,    0.2f), \
    PARAM_DEFINE_FLOAT(x##RDRIFT,   0), \
    PARAM_DEFINE_INT8 (x##RDISABLE, 0), \
    PARAM_DEFINE_FLOAT(x##RGLITCH,  0), \
    PARAM_DEFINE_INT8 (x##RFREEZE,  0), \
    PARAM_DEFINE_INT16(x##RDELAY,   0), \
    PARAM_DEFINE_FLOAT(x##RWCF_FWD, 0.0f), \
    PARAM_DEFINE_FLOAT(x##RWCF_BAK, 0.0f), \
    PARAM_DEFINE_FLOAT(x##RWCF_RGT, 0.0f), \
    PARAM_DEFINE_FLOAT(x##RWCF_LFT, 0.0f)

#define SIM_BARO_PARAM_LINK_VARIABLE(baro, x) \
    param_link_variable(PARAM_ID(SIM3, x##RND),      &baro.noise); \
    param_link_variable(PARAM_ID(SIM3, x##RDRIFT),   &baro.drift); \
    param_link_variable(PARAM_ID(SIM3, x##RDISABLE), &baro.disable); \
    param_link_variable(PARAM_ID(SIM3, x##RGLITCH),  &baro.glitch); \
    param_link_variable(PARAM_ID(SIM3, x##RFREEZE),  &baro.freeze); \
    param_link_variable(PARAM_ID(SIM3, x##RDELAY),   &baro.delay); \
    param_link_variable(PARAM_ID(SIM3, x##RWCF_FWD), &baro.wcof_xp); \
    param_link_variable(PARAM_ID(SIM3, x##RWCF_BAK), &baro.wcof_xn); \
    param_link_variable(PARAM_ID(SIM3, x##RWCF_RGT), &baro.wcof_yp); \
    param_link_variable(PARAM_ID(SIM3, x##RWCF_LFT), &baro.wcof_yn)

#define AIRSPEED_GROUPINFO(x) PARAM_DEFINE_FLOAT(x##RND,     2.0f), \
    PARAM_DEFINE_FLOAT(x##OFS,                               2013), \
    PARAM_DEFINE_FLOAT(x##FAIL,                              0), \
    PARAM_DEFINE_FLOAT(x##FAILP,                             0), \
    PARAM_DEFINE_FLOAT(x##PITOT,                             0), \
    PARAM_DEFINE_INT8 (x##SIGN,                              0), \
    PARAM_DEFINE_FLOAT(x##RATIO,                             1.99f)

#define SIM_AIRSPEED_PARAM_LINK_VARIABLE(airspeed, x) \
    param_link_variable(PARAM_ID(SIM3, x##RND),     &airspeed.noise); \
    param_link_variable(PARAM_ID(SIM3, x##OFS),     &airspeed.offset); \
    param_link_variable(PARAM_ID(SIM3, x##FAIL),    &airspeed.fail); \
    param_link_variable(PARAM_ID(SIM3, x##FAILP),   &airspeed.fail_pressure); \
    param_link_variable(PARAM_ID(SIM3, x##PITOT),   &airspeed.fail_pitot_pressure); \
    param_link_variable(PARAM_ID(SIM3, x##SIGN),    &airspeed.signflip); \
    param_link_variable(PARAM_ID(SIM3, x##RATIO),   &airspeed.ratio)

#define SIM_GPS_GROUPINFO(x, disable) PARAM_DEFINE_INT8(x##DISABLE,     disable), \
    PARAM_DEFINE_INT16(x##LAG_MS,                              100), \
    PARAM_DEFINE_INT8 (x##TYPE,                                SIM_GPS_UBLOX), \
    PARAM_DEFINE_FLOAT(x##BYTELOS,                             0), \
    PARAM_DEFINE_INT8 (x##NUMSATS,                             10), \
    PARAM_DEFINE_FLOAT(x##GLTCH_X,                             0), \
    PARAM_DEFINE_FLOAT(x##GLTCH_Y,                             0), \
    PARAM_DEFINE_FLOAT(x##GLTCH_Z,                             0), \
    PARAM_DEFINE_INT8 (x##HZ,                                  5), \
    PARAM_DEFINE_FLOAT(x##DRFTALT,                             0), \
    PARAM_DEFINE_FLOAT(x##POS_X,                               0), \
    PARAM_DEFINE_FLOAT(x##POS_Y,                               0), \
    PARAM_DEFINE_FLOAT(x##POS_Z,                               0), \
    PARAM_DEFINE_FLOAT(x##NOISE,                               0), \
    PARAM_DEFINE_INT16(x##LCKTIME,                             0), \
    PARAM_DEFINE_INT16(x##ALT_OFS,                             0), \
    PARAM_DEFINE_INT8 (x##HDG,                                 GPS_HEADING_NONE), \
    PARAM_DEFINE_FLOAT(x##ACC,                                 0.3f), \
    PARAM_DEFINE_FLOAT(x##VERR_X,                              0), \
    PARAM_DEFINE_FLOAT(x##VERR_Y,                              0), \
    PARAM_DEFINE_FLOAT(x##VERR_Z,                              0)

#define SIM_GPS_PARAM_LINK_VARIABLE(sim, xn, n) \
    param_link_variable(PARAM_ID(SIM_GPS, xn##DISABLE),     &sim->gps_disable[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##LAG_MS),      &sim->gps_delay_ms[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##TYPE),        &sim->gps_type[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##BYTELOS),     &sim->gps_byteloss[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##NUMSATS),     &sim->gps_numsats[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##GLTCH_X),     &sim->gps_glitch[n].x); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##GLTCH_Y),     &sim->gps_glitch[n].y); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##GLTCH_Z),     &sim->gps_glitch[n].z); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##HZ),          &sim->gps_hertz[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##DRFTALT),     &sim->gps_drift_alt[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##POS_X),       &sim->gps_pos_offset[n].x); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##POS_Y),       &sim->gps_pos_offset[n].y); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##POS_Z),       &sim->gps_pos_offset[n].z); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##NOISE),       &sim->gps_noise[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##LCKTIME),     &sim->gps_lock_time[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##ALT_OFS),     &sim->gps_alt_offset[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##HDG),         &sim->gps_hdg_enabled[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##ACC),         &sim->gps_accuracy[n]); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##VERR_X),      &sim->gps_vel_err[n].x); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##VERR_Y),      &sim->gps_vel_err[n].y); \
    param_link_variable(PARAM_ID(SIM_GPS, xn##VERR_Z),      &sim->gps_vel_err[n].z)


#define SIM_MAG_SUBGROUPINFO(x) \
    PARAM_DEFINE_FLOAT(x##OFS_X,                               0), \
    PARAM_DEFINE_FLOAT(x##OFS_Y,                               0), \
    PARAM_DEFINE_FLOAT(x##OFS_Z,                               0), \
    PARAM_DEFINE_FLOAT(x##DIA_X,                               0), \
    PARAM_DEFINE_FLOAT(x##DIA_Y,                               0), \
    PARAM_DEFINE_FLOAT(x##DIA_Z,                               0), \
    PARAM_DEFINE_FLOAT(x##ODI_X,                               0), \
    PARAM_DEFINE_FLOAT(x##ODI_Y,                               0), \
    PARAM_DEFINE_FLOAT(x##ODI_Z,                               0), \
    PARAM_DEFINE_INT8 (x##ORIENT,                              0), \
    PARAM_DEFINE_INT8 (x##FAIL,                                0), \
    PARAM_DEFINE_FLOAT(x##SCALING,                             1.0f)

#define SIM_MAG_SUBPARAM_LINK_VARIABLE(sim, xn, n) \
    param_link_variable(PARAM_ID(SIM_MAG, xn##OFS_X),     &sim->mag_ofs[n].x); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##OFS_Y),     &sim->mag_ofs[n].y); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##OFS_Z),     &sim->mag_ofs[n].z); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##DIA_X),     &sim->mag_diag[n].x); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##DIA_Y),     &sim->mag_diag[n].y); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##DIA_Z),     &sim->mag_diag[n].z); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##ODI_X),     &sim->mag_offdiag[n].x); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##ODI_Y),     &sim->mag_offdiag[n].y); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##ODI_Z),     &sim->mag_offdiag[n].z); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##ORIENT),    &sim->mag_orient[n]); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##FAIL),      &sim->mag_fail[n]); \
    param_link_variable(PARAM_ID(SIM_MAG, xn##SCALING),   &sim->mag_scaling[n])

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
// table of user settable parameters
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(SIM_DRIFT_SPEED,       0.05f),
    PARAM_DEFINE_FLOAT(SIM_DRIFT_TIME,        5),
    PARAM_DEFINE_FLOAT(SIM_ENGINE_MUL,        1),
    PARAM_DEFINE_FLOAT(SIM_WIND_SPD,          0),
    PARAM_DEFINE_FLOAT(SIM_WIND_DIR,          180),
    PARAM_DEFINE_FLOAT(SIM_WIND_TURB,         0),
    PARAM_DEFINE_FLOAT(SIM_SERVO_SPEED,       0.14f),
    PARAM_DEFINE_INT8 (SIM_SONAR_ROT,         ROTATION_PITCH_270),
    PARAM_DEFINE_FLOAT(SIM_BATT_VOLTAGE,      12.6f),
    PARAM_DEFINE_FLOAT(SIM_BATT_CAP_AH,       0),
    PARAM_DEFINE_FLOAT(SIM_SONAR_GLITCH,      0),
    PARAM_DEFINE_FLOAT(SIM_SONAR_RND,         0),
    PARAM_DEFINE_INT8 (SIM_RC_FAIL,           0),
    PARAM_DEFINE_INT8 (SIM_FLOAT_EXCEPT,      1),
    PARAM_DEFINE_FLOAT(SIM_SONAR_SCALE,       12.1212f),
    PARAM_DEFINE_INT8 (SIM_FLOW_ENABLE,       0),
    PARAM_DEFINE_INT8 (SIM_TERRAIN,           1),
    PARAM_DEFINE_INT16(SIM_FLOW_RATE,         10),
    PARAM_DEFINE_INT8 (SIM_FLOW_DELAY,        0),
    PARAM_DEFINE_INT16(SIM_ADSB_COUNT,        -1),
    PARAM_DEFINE_FLOAT(SIM_ADSB_RADIUS,       10000),
    PARAM_DEFINE_FLOAT(SIM_ADSB_ALT,          1000),
    PARAM_DEFINE_INT16(SIM_PIN_MASK,          0),
    PARAM_DEFINE_INT8 (SIM_ADSB_TX,           0),
    PARAM_DEFINE_FLOAT(SIM_SPEEDUP,           -1),

    PARAM_DEFINE_FLOAT(SIM_IMU_POS_X,         0),
    PARAM_DEFINE_FLOAT(SIM_IMU_POS_Y,         0),
    PARAM_DEFINE_FLOAT(SIM_IMU_POS_Z,         0),

    PARAM_DEFINE_FLOAT(SIM_SONAR_POS_X,       0),
    PARAM_DEFINE_FLOAT(SIM_SONAR_POS_Y,       0),
    PARAM_DEFINE_FLOAT(SIM_SONAR_POS_Z,       0),

    PARAM_DEFINE_FLOAT(SIM_FLOW_POS_X,        0),
    PARAM_DEFINE_FLOAT(SIM_FLOW_POS_Y,        0),
    PARAM_DEFINE_FLOAT(SIM_FLOW_POS_Z,        0),

    PARAM_DEFINE_INT8 (SIM_ENGINE_FAIL,       0),
};
PARAM_GROUP_DEFINE(SIM, var_info);

// second table of user settable parameters for SITL. 
static param_t var_info2[] = {
    PARAM_DEFINE_FLOAT(SIM_TEMP_START,            25),
    PARAM_DEFINE_FLOAT(SIM_TEMP_BRD_OFF,          20),
    PARAM_DEFINE_FLOAT(SIM_TEMP_TCONST,           30),
    PARAM_DEFINE_FLOAT(SIM_TEMP_BFACTOR,          0),

    PARAM_DEFINE_FLOAT(SIM_WIND_DIR_Z,            0),
    PARAM_DEFINE_INT8 (SIM_WIND_T,                WIND_TYPE_SQRT),
    PARAM_DEFINE_FLOAT(SIM_WIND_T_ALT,            60),
    PARAM_DEFINE_FLOAT(SIM_WIND_T_COEF,           0.01f),
    PARAM_DEFINE_INT8 (SIM_RC_CHANCOUNT,          16),

    // weight on wheels pin
    PARAM_DEFINE_INT8 (SIM_WOW_PIN,               -1),

    // vibration frequencies on each axis
    PARAM_DEFINE_FLOAT(SIM_VIB_FREQ_X,             0),
    PARAM_DEFINE_FLOAT(SIM_VIB_FREQ_Y,             0),
    PARAM_DEFINE_FLOAT(SIM_VIB_FREQ_Z,             0),

    // enable bandwidth limitting on telemetry ports:
    PARAM_DEFINE_INT8 (SIM_BAUDLIMIT_EN,           0),

    // apply a force to the vehicle over a period of time:
    PARAM_DEFINE_FLOAT(SIM_SHOVE_X,                0),
    PARAM_DEFINE_FLOAT(SIM_SHOVE_Y,                0),
    PARAM_DEFINE_FLOAT(SIM_SHOVE_Z,                0),
    PARAM_DEFINE_INT32(SIM_SHOVE_TIME,             0),
    
    // optical flow sensor measurement noise in rad/sec
    PARAM_DEFINE_FLOAT(SIM_FLOW_RND,               0.05f),

    PARAM_DEFINE_FLOAT(SIM_TWIST_X,                0),
    PARAM_DEFINE_FLOAT(SIM_TWIST_Y,                0),
    PARAM_DEFINE_FLOAT(SIM_TWIST_Z,                0),
    PARAM_DEFINE_INT32(SIM_TWIST_TIME,             0),

    PARAM_DEFINE_INT8 (SIM_GND_BEHAV,              -1),

    // sailboat wave and tide simulation parameters
    PARAM_DEFINE_INT8 (SIM_WAVE_ENABLE,            0),
    PARAM_DEFINE_FLOAT(SIM_WAVE_LENGTH,            10.0f),
    PARAM_DEFINE_FLOAT(SIM_WAVE_AMP,               0.5f),
    PARAM_DEFINE_FLOAT(SIM_WAVE_DIR,               0.0f),
    PARAM_DEFINE_FLOAT(SIM_WAVE_SPEED,             0.5f),

    PARAM_DEFINE_FLOAT(SIM_TIDE_DIR,               0.0f),
    PARAM_DEFINE_FLOAT(SIM_TIDE_SPEED,             0.0f),

    // the following coordinates are for CMAC, in Canberra
    PARAM_DEFINE_FLOAT(SIM_OPOS_LAT,               -35.363261f),
    PARAM_DEFINE_FLOAT(SIM_OPOS_LNG,               149.165230f),
    PARAM_DEFINE_FLOAT(SIM_OPOS_ALT,               584.0f),
    PARAM_DEFINE_FLOAT(SIM_OPOS_HDG,               353.0f),

    // extra delay per main loop
    PARAM_DEFINE_INT32(SIM_LOOP_DELAY,             0),

    PARAM_DEFINE_INT8 (SIM_EFI_TYPE,               EFI_TYPE_NONE),

    PARAM_DEFINE_INT8 (SIM_SAFETY_STATE,           0),

    // motor harmonics
    PARAM_DEFINE_INT16(SIM_VIB_MOT_HMNC,           1),

    // motor mask, allowing external simulators to mark motors
    PARAM_DEFINE_INT32(SIM_VIB_MOT_MASK,           0),
    
    // max motor vibration frequency
    PARAM_DEFINE_FLOAT(SIM_VIB_MOT_MAX,            0.0f),
    // minimum throttle for simulated ins noise
    PARAM_DEFINE_FLOAT(SIM_INS_THR_MIN,            0.1f),
    // amplitude scaling of motor noise relative to gyro/accel noise
    PARAM_DEFINE_FLOAT(SIM_VIB_MOT_MULT,           1.0f),
};
PARAM_GROUP_DEFINE(SIM2, var_info2);

// third table of user settable parameters for SITL. 
static param_t var_info3[] = {
    PARAM_DEFINE_INT8 (SIM_ODOM_ENABLE,            0),

    PARAM_DEFINE_INT8 (SIM_LED_LAYOUT,             0),

    // Scenario for thermalling simulation, for soaring
    PARAM_DEFINE_INT8 (SIM_THML_SCENARI,           0),

    // vicon sensor position (position offsets in body frame)
    PARAM_DEFINE_FLOAT(SIM_VICON_POS_X,            0),
    PARAM_DEFINE_FLOAT(SIM_VICON_POS_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_VICON_POS_Z,            0),

    // Buyoancy for submarines
    PARAM_DEFINE_FLOAT(SIM_BUOYANCY,               1),

    // vicon glitch in NED frame
    PARAM_DEFINE_FLOAT(SIM_VICON_GLIT_X,           0),
    PARAM_DEFINE_FLOAT(SIM_VICON_GLIT_Y,           0),
    PARAM_DEFINE_FLOAT(SIM_VICON_GLIT_Z,           0),

    // vicon failure
    PARAM_DEFINE_INT8 (SIM_VICON_FAIL,             0),

    // vicon yaw (in earth frame)
    PARAM_DEFINE_INT16(SIM_VICON_YAW,              0),

    // vicon yaw error in degrees (added to reported yaw sent to vehicle)
    PARAM_DEFINE_INT16(SIM_VICON_YAWERR,           0),

    // vicon message type mask
    PARAM_DEFINE_INT8 (SIM_VICON_TMASK,            3),

    // vicon velocity glitch in NED frame
    PARAM_DEFINE_FLOAT(SIM_VICON_VGLI_X,           0),
    PARAM_DEFINE_FLOAT(SIM_VICON_VGLI_Y,           0),
    PARAM_DEFINE_FLOAT(SIM_VICON_VGLI_Z,           0),

    PARAM_DEFINE_INT16(SIM_RATE_HZ,                500),

    // count of simulated IMUs
    PARAM_DEFINE_INT8 (SIM_IMU_COUNT,              2),

    // user settable barometer parameters
    PARAM_DEFINE_INT8 (SIM_BARO_COUNT,             2),

    BARO_GROUPINFO(SIM_BARO_),
#if BARO_MAX_INSTANCES > 1
    BARO_GROUPINFO(SIM_BAR2_),
#endif
#if BARO_MAX_INSTANCES > 2
    BARO_GROUPINFO(SIM_BAR3_),
#endif

    // user settable parameters for the 1st barometer
    // @Param: BARO_RND
    // @DisplayName: Baro Noise
    // @Description: Amount of (evenly-distributed) noise injected into the 1st baro
    // @Units: m
    // @User: Advanced

    // @Param: BARO_GLITCH
    // @DisplayName: Baro Glitch
    // @Description: Glitch for 1st baro
    // @Units: m
    // @User: Advanced

    // user settable parameters for the 2nd barometer
    // @Param: BAR2_RND
    // @DisplayName: Baro2 Noise
    // @Description: Amount of (evenly-distributed) noise injected into the 2nd baro
    // @Units: m
    // @User: Advanced

    // @Param: BAR2_GLITCH
    // @DisplayName: Baro2 Glitch
    // @Description: Glitch for 2nd baro
    // @Units: m
    // @User: Advanced

    // user settable parameters for the 3rd barometer
    // @Param: BAR3_RND
    // @DisplayName: Baro3 Noise
    // @Description: Amount of (evenly-distributed) noise injected into the 3rd baro
    // @Units: m
    // @User: Advanced

    // @Param: BAR3_GLITCH
    // @DisplayName: Baro3 Glitch
    // @Description: Glitch for 2nd baro
    // @Units: m
    // @User: Advanced

    PARAM_DEFINE_INT8 (SIM_ESC_TELEM,              1),

    PARAM_DEFINE_FLOAT(SIM_ESC_ARM_RPM,            0.0f),

    AIRSPEED_GROUPINFO(SIM_ARSPD_),
#if AIRSPEED_MAX_SENSORS > 1
    AIRSPEED_GROUPINFO(SIM_ARSPD2_),
#endif
};
PARAM_GROUP_DEFINE(SIM3, var_info3);

// GPS SITL parameters
static param_t var_gps[] = {
    SIM_GPS_GROUPINFO(SIM_GPS_,  0),
    SIM_GPS_GROUPINFO(SIM_GPS2_, 1),

    PARAM_DEFINE_FLOAT(SIM_INIT_LAT_OFS,  0),
    PARAM_DEFINE_FLOAT(SIM_INIT_LON_OFS,  0),
    PARAM_DEFINE_FLOAT(SIM_INIT_ALT_OFS,  0),
};
PARAM_GROUP_DEFINE(SIM_GPS, var_gps);

// Mag SITL parameters
static param_t var_mag[] = {
    PARAM_DEFINE_FLOAT(SIM_MAG_RND,                0),
    PARAM_DEFINE_FLOAT(SIM_MAG_MOT_X,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_MOT_Y,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_MOT_Z,              0),
    PARAM_DEFINE_INT16(SIM_MAG_DELAY,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_ALY_X,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_ALY_Y,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_ALY_Z,              0),
    PARAM_DEFINE_FLOAT(SIM_MAG_ALY_HGT,            1.0f),
    PARAM_DEFINE_INT32(SIM_MAG1_DEVID,             97539),
    PARAM_DEFINE_INT32(SIM_MAG2_DEVID,             131874),
#if MAX_CONNECTED_MAGS > 2
    PARAM_DEFINE_INT32(SIM_MAG3_DEVID,             263178),
#endif
#if MAX_CONNECTED_MAGS > 3
    PARAM_DEFINE_INT32(SIM_MAG4_DEVID,             97283),
#endif
#if MAX_CONNECTED_MAGS > 4
    PARAM_DEFINE_INT32(SIM_MAG5_DEVID,             97795),
#endif
#if MAX_CONNECTED_MAGS > 5
    PARAM_DEFINE_INT32(SIM_MAG6_DEVID,             98051),
#endif
#if MAX_CONNECTED_MAGS > 6
    PARAM_DEFINE_INT32(SIM_MAG7_DEVID,             0),
#endif
#if MAX_CONNECTED_MAGS > 7
    PARAM_DEFINE_INT32(SIM_MAG8_DEVID,             0),
#endif
    SIM_MAG_SUBGROUPINFO(SIM_MAG_),
#if HAL_COMPASS_MAX_SENSORS > 1
    SIM_MAG_SUBGROUPINFO(SIM_MAG2_),
#endif
#if HAL_COMPASS_MAX_SENSORS > 2
    SIM_MAG_SUBGROUPINFO(SIM_MAG3_),
#endif
};
PARAM_GROUP_DEFINE(SIM_MAG, var_mag);

// INS SITL parameters
static param_t var_ins[] = {
    PARAM_DEFINE_FLOAT(SIM_ACC1_BIAS_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC1_BIAS_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC1_BIAS_Z,            0),
#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_ACC2_BIAS_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC2_BIAS_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC2_BIAS_Z,            0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_ACC3_BIAS_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC3_BIAS_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC3_BIAS_Z,            0),
#endif
    PARAM_DEFINE_FLOAT(SIM_GYR1_RND,               0),
#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_GYR2_RND,               0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_GYR3_RND,               0),
#endif
    PARAM_DEFINE_FLOAT(SIM_ACC1_RND,               0),
#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_ACC2_RND,               0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_ACC3_RND,               0),
#endif
    PARAM_DEFINE_FLOAT(SIM_GYR1_SCALE_X,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR1_SCALE_Y,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR1_SCALE_Z,           0),
#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_GYR2_SCALE_X,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR2_SCALE_Y,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR2_SCALE_Z,           0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_GYR3_SCALE_X,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR3_SCALE_Y,           0),
    PARAM_DEFINE_FLOAT(SIM_GYR3_SCALE_Z,           0),
#endif
    PARAM_DEFINE_FLOAT(SIM_ACCEL1_FAIL,            0),
#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_ACCEL2_FAIL,            0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_ACCEL3_FAIL,            0),
#endif
    PARAM_DEFINE_INT8 (SIM_GYR_FAIL_MSK,           0),
    PARAM_DEFINE_INT8 (SIM_ACC_FAIL_MSK,           0),

    PARAM_DEFINE_FLOAT(SIM_ACC1_SCAL_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC1_SCAL_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC1_SCAL_Z,            0),

#if INS_MAX_INSTANCES > 1
    PARAM_DEFINE_FLOAT(SIM_ACC2_SCAL_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC2_SCAL_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC2_SCAL_Z,            0),
#endif
#if INS_MAX_INSTANCES > 2
    PARAM_DEFINE_FLOAT(SIM_ACC3_SCAL_X,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC3_SCAL_Y,            0),
    PARAM_DEFINE_FLOAT(SIM_ACC3_SCAL_Z,            0),
#endif
    PARAM_DEFINE_FLOAT(SIM_ACC_TRIM_X,             0),
    PARAM_DEFINE_FLOAT(SIM_ACC_TRIM_Y,             0),
    PARAM_DEFINE_FLOAT(SIM_ACC_TRIM_Z,             0),

    // @Param: SAIL_TYPE
    // @DisplayName: Sailboat simulation sail type
    // @Description: 0: mainsail with sheet, 1: directly actuated wing
    PARAM_DEFINE_INT8 (SIM_SAIL_TYPE,              0),

    // @Param: JSON_MASTER
    // @DisplayName: JSON master instance
    // @Description: the instance number to  take servos from
    PARAM_DEFINE_INT8 (SIM_JSON_MASTER,            0),

    // @Param: OH_MASK
    // @DisplayName: SIM-on_hardware Output Enable Mask
    // @Description: channels which are passed through to actual hardware when running on actual hardware
    PARAM_DEFINE_INT32(SIM_OH_MASK,                0),
};
PARAM_GROUP_DEFINE(SIM_INS, var_ins);

const Location post_origin  = {
    .lat = 518752066,
    .lng = 146487830,
    .alt = 0,
    .relative_alt  = 0,
    .loiter_ccw    = 0,
    .terrain_alt   = 0,
    .origin_alt    = 0,
    .loiter_xtrack = 0,
};

static struct SIM _singleton;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
sim_t sim_get_singleton()
{
    return &_singleton;
}

void sim_ctor(sim_t sim)
{
    param_link_variable(PARAM_ID(SIM, SIM_DRIFT_SPEED), &sim->drift_speed);
    param_link_variable(PARAM_ID(SIM, SIM_DRIFT_TIME),  &sim->drift_time);
    param_link_variable(PARAM_ID(SIM, SIM_ENGINE_MUL),  &sim->engine_mul);
    param_link_variable(PARAM_ID(SIM, SIM_WIND_SPD),    &sim->wind_speed);
    param_link_variable(PARAM_ID(SIM, SIM_WIND_DIR),    &sim->wind_direction);
    param_link_variable(PARAM_ID(SIM, SIM_WIND_TURB),   &sim->wind_turbulance);
    param_link_variable(PARAM_ID(SIM, SIM_SERVO_SPEED), &sim->servo_speed);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_ROT),   &sim->sonar_rot);
    param_link_variable(PARAM_ID(SIM, SIM_BATT_VOLTAGE),&sim->batt_voltage);
    param_link_variable(PARAM_ID(SIM, SIM_BATT_CAP_AH), &sim->batt_capacity_ah);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_GLITCH),&sim->sonar_glitch);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_RND),   &sim->sonar_noise);
    param_link_variable(PARAM_ID(SIM, SIM_RC_FAIL),     &sim->rc_fail);
    param_link_variable(PARAM_ID(SIM, SIM_FLOAT_EXCEPT),&sim->float_exception);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_SCALE), &sim->sonar_scale);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_ENABLE), &sim->flow_enable);
    param_link_variable(PARAM_ID(SIM, SIM_TERRAIN),     &sim->terrain_enable);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_RATE),   &sim->flow_rate);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_DELAY),  &sim->flow_delay);
    param_link_variable(PARAM_ID(SIM, SIM_ADSB_COUNT),  &sim->adsb_plane_count);
    param_link_variable(PARAM_ID(SIM, SIM_ADSB_RADIUS), &sim->adsb_radius_m);
    param_link_variable(PARAM_ID(SIM, SIM_ADSB_ALT),    &sim->adsb_altitude_m);
    param_link_variable(PARAM_ID(SIM, SIM_PIN_MASK),    &sim->pin_mask);
    param_link_variable(PARAM_ID(SIM, SIM_ADSB_TX),     &sim->adsb_tx);
    param_link_variable(PARAM_ID(SIM, SIM_SPEEDUP),     &sim->speedup);
    param_link_variable(PARAM_ID(SIM, SIM_IMU_POS_X),   &sim->imu_pos_offset.x);
    param_link_variable(PARAM_ID(SIM, SIM_IMU_POS_Y),   &sim->imu_pos_offset.y);
    param_link_variable(PARAM_ID(SIM, SIM_IMU_POS_Z),   &sim->imu_pos_offset.z);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_POS_X), &sim->rngfnd_pos_offset.x);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_POS_Y), &sim->rngfnd_pos_offset.y);
    param_link_variable(PARAM_ID(SIM, SIM_SONAR_POS_Z), &sim->rngfnd_pos_offset.z);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_POS_X),  &sim->optflow_pos_offset.x);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_POS_Y),  &sim->optflow_pos_offset.y);
    param_link_variable(PARAM_ID(SIM, SIM_FLOW_POS_Z),  &sim->optflow_pos_offset.z);
    param_link_variable(PARAM_ID(SIM, SIM_ENGINE_FAIL), &sim->engine_fail);

    param_link_variable(PARAM_ID(SIM2, SIM_TEMP_START),   &sim->temp_start);
    param_link_variable(PARAM_ID(SIM2, SIM_TEMP_BRD_OFF), &sim->temp_board_offset);
    param_link_variable(PARAM_ID(SIM2, SIM_TEMP_TCONST),  &sim->temp_tconst);
    param_link_variable(PARAM_ID(SIM2, SIM_TEMP_BFACTOR), &sim->temp_baro_factor);
    param_link_variable(PARAM_ID(SIM2, SIM_WIND_DIR_Z),   &sim->wind_dir_z);
    param_link_variable(PARAM_ID(SIM2, SIM_WIND_T),       &sim->wind_type);
    param_link_variable(PARAM_ID(SIM2, SIM_WIND_T_ALT),   &sim->wind_type_alt);
    param_link_variable(PARAM_ID(SIM2, SIM_WIND_T_COEF),  &sim->wind_type_coef);
    param_link_variable(PARAM_ID(SIM2, SIM_RC_CHANCOUNT), &sim->rc_chancount);
    param_link_variable(PARAM_ID(SIM2, SIM_WOW_PIN),      &sim->wow_pin);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_FREQ_X),   &sim->vibe_freq.x);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_FREQ_Y),   &sim->vibe_freq.y);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_FREQ_Z),   &sim->vibe_freq.z);
    param_link_variable(PARAM_ID(SIM2, SIM_BAUDLIMIT_EN), &sim->telem_baudlimit_enable);
    param_link_variable(PARAM_ID(SIM2, SIM_SHOVE_X),      &sim->shove.x);
    param_link_variable(PARAM_ID(SIM2, SIM_SHOVE_Y),      &sim->shove.y);
    param_link_variable(PARAM_ID(SIM2, SIM_SHOVE_Z),      &sim->shove.z);
    param_link_variable(PARAM_ID(SIM2, SIM_SHOVE_TIME),   &sim->shove.t);
    param_link_variable(PARAM_ID(SIM2, SIM_FLOW_RND),     &sim->flow_noise);
    param_link_variable(PARAM_ID(SIM2, SIM_TWIST_X),      &sim->twist.x);
    param_link_variable(PARAM_ID(SIM2, SIM_TWIST_Y),      &sim->twist.y);
    param_link_variable(PARAM_ID(SIM2, SIM_TWIST_Z),      &sim->twist.z);
    param_link_variable(PARAM_ID(SIM2, SIM_TWIST_TIME),   &sim->twist.t);
    param_link_variable(PARAM_ID(SIM2, SIM_GND_BEHAV),    &sim->gnd_behav);
    param_link_variable(PARAM_ID(SIM2, SIM_WAVE_ENABLE),  &sim->wave.enable);
    param_link_variable(PARAM_ID(SIM2, SIM_WAVE_LENGTH),  &sim->wave.length);
    param_link_variable(PARAM_ID(SIM2, SIM_WAVE_AMP),     &sim->wave.amp);
    param_link_variable(PARAM_ID(SIM2, SIM_WAVE_DIR),     &sim->wave.direction);
    param_link_variable(PARAM_ID(SIM2, SIM_WAVE_SPEED),   &sim->wave.speed);
    param_link_variable(PARAM_ID(SIM2, SIM_TIDE_DIR),     &sim->tide.direction);
    param_link_variable(PARAM_ID(SIM2, SIM_TIDE_SPEED),   &sim->tide.speed);
    param_link_variable(PARAM_ID(SIM2, SIM_OPOS_LAT),     &sim->opos.lat);
    param_link_variable(PARAM_ID(SIM2, SIM_OPOS_LNG),     &sim->opos.lng);
    param_link_variable(PARAM_ID(SIM2, SIM_OPOS_ALT),     &sim->opos.alt);
    param_link_variable(PARAM_ID(SIM2, SIM_OPOS_HDG),     &sim->opos.hdg);
    param_link_variable(PARAM_ID(SIM2, SIM_LOOP_DELAY),   &sim->loop_delay);
    param_link_variable(PARAM_ID(SIM2, SIM_EFI_TYPE),     &sim->efi_type);
    param_link_variable(PARAM_ID(SIM2, SIM_SAFETY_STATE), &sim->_safety_switch_state);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_MOT_HMNC), &sim->vibe_motor_harmonics);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_MOT_MASK), &sim->vibe_motor_mask);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_MOT_MAX),  &sim->vibe_motor);
    param_link_variable(PARAM_ID(SIM2, SIM_INS_THR_MIN),  &sim->ins_noise_throttle_min);
    param_link_variable(PARAM_ID(SIM2, SIM_VIB_MOT_MULT), &sim->vibe_motor_scale);

    param_link_variable(PARAM_ID(SIM3, SIM_ODOM_ENABLE),  &sim->odom_enable);
    param_link_variable(PARAM_ID(SIM3, SIM_LED_LAYOUT),   &sim->led_layout);
    param_link_variable(PARAM_ID(SIM3, SIM_THML_SCENARI), &sim->thermal_scenario);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_POS_X),  &sim->vicon_pos_offset.x);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_POS_Y),  &sim->vicon_pos_offset.y);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_POS_Z),  &sim->vicon_pos_offset.z);
    param_link_variable(PARAM_ID(SIM3, SIM_BUOYANCY),     &sim->buoyancy);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_GLIT_X), &sim->vicon_glitch.x);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_GLIT_Y), &sim->vicon_glitch.y);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_GLIT_Z), &sim->vicon_glitch.z);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_FAIL),   &sim->vicon_fail);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_YAW),    &sim->vicon_yaw);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_YAWERR), &sim->vicon_yaw_error);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_TMASK),  &sim->vicon_type_mask);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_VGLI_X), &sim->vicon_vel_glitch.x);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_VGLI_Y), &sim->vicon_vel_glitch.y);
    param_link_variable(PARAM_ID(SIM3, SIM_VICON_VGLI_Z), &sim->vicon_vel_glitch.z);
    param_link_variable(PARAM_ID(SIM3, SIM_RATE_HZ),      &sim->loop_rate_hz);
    param_link_variable(PARAM_ID(SIM3, SIM_IMU_COUNT),    &sim->imu_count);
    param_link_variable(PARAM_ID(SIM3, SIM_BARO_COUNT),   &sim->baro_count);

    SIM_BARO_PARAM_LINK_VARIABLE(sim->baro[0], SIM_BARO_);
#if BARO_MAX_INSTANCES > 1
    SIM_BARO_PARAM_LINK_VARIABLE(sim->baro[1], SIM_BAR2_);
#endif
#if BARO_MAX_INSTANCES > 2
    SIM_BARO_PARAM_LINK_VARIABLE(sim->baro[2], SIM_BAR3_);
#endif

    param_link_variable(PARAM_ID(SIM3, SIM_ESC_TELEM),    &sim->esc_telem);
    param_link_variable(PARAM_ID(SIM3, SIM_ESC_ARM_RPM),  &sim->esc_rpm_armed);

    SIM_AIRSPEED_PARAM_LINK_VARIABLE(sim->airspeed[0], SIM_ARSPD_);
#if AIRSPEED_MAX_SENSORS > 1
    SIM_AIRSPEED_PARAM_LINK_VARIABLE(sim->airspeed[1], SIM_ARSPD2_);
#endif

    SIM_GPS_PARAM_LINK_VARIABLE(sim, SIM_GPS_, 0);
    SIM_GPS_PARAM_LINK_VARIABLE(sim, SIM_GPS2_, 1);

    param_link_variable(PARAM_ID(SIM_GPS, SIM_INIT_LAT_OFS),    &sim->gps_init_lat_ofs);
    param_link_variable(PARAM_ID(SIM_GPS, SIM_INIT_LON_OFS),    &sim->gps_init_lon_ofs);
    param_link_variable(PARAM_ID(SIM_GPS, SIM_INIT_ALT_OFS),    &sim->gps_init_alt_ofs);

    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_RND),         &sim->mag_noise),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_MOT_X),       &sim->mag_mot.x),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_MOT_Y),       &sim->mag_mot.y),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_MOT_Z),       &sim->mag_mot.z),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_DELAY),       &sim->mag_delay),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_ALY_X),       &sim->mag_anomaly_ned.x),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_ALY_Y),       &sim->mag_anomaly_ned.y),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_ALY_Z),       &sim->mag_anomaly_ned.z),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG_ALY_HGT),     &sim->mag_anomaly_hgt),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG1_DEVID),      &sim->mag_devid[0]),
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG2_DEVID),      &sim->mag_devid[1]),
#if MAX_CONNECTED_MAGS > 2
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG3_DEVID),      &sim->mag_devid[2]),
#endif
#if MAX_CONNECTED_MAGS > 3
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG4_DEVID),      &sim->mag_devid[3]),
#endif
#if MAX_CONNECTED_MAGS > 4
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG5_DEVID),      &sim->mag_devid[4]),
#endif
#if MAX_CONNECTED_MAGS > 5
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG6_DEVID),      &sim->mag_devid[5]),
#endif
#if MAX_CONNECTED_MAGS > 6
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG7_DEVID),      &sim->mag_devid[6]),
#endif
#if MAX_CONNECTED_MAGS > 7
    param_link_variable(PARAM_ID(SIM_MAG, SIM_MAG8_DEVID),      &sim->mag_devid[7]),
#endif

    SIM_MAG_SUBPARAM_LINK_VARIABLE(sim, SIM_MAG_,  0);
#if HAL_COMPASS_MAX_SENSORS > 1
    SIM_MAG_SUBPARAM_LINK_VARIABLE(sim, SIM_MAG2_, 1);
#endif
#if HAL_COMPASS_MAX_SENSORS > 2
    SIM_MAG_SUBPARAM_LINK_VARIABLE(sim, SIM_MAG3_, 2);
#endif

    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_BIAS_X),      &sim->accel_bias[0].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_BIAS_Y),      &sim->accel_bias[0].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_BIAS_Z),      &sim->accel_bias[0].z);
#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_BIAS_X),      &sim->accel_bias[1].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_BIAS_Y),      &sim->accel_bias[1].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_BIAS_Z),      &sim->accel_bias[1].z);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_BIAS_X),      &sim->accel_bias[2].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_BIAS_Y),      &sim->accel_bias[2].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_BIAS_Z),      &sim->accel_bias[2].z);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR1_RND),         &sim->gyro_noise[0]);
#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR2_RND),         &sim->gyro_noise[1]);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR3_RND),         &sim->gyro_noise[2]);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_RND),         &sim->accel_noise[0]);
#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_RND),         &sim->accel_noise[1]);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_RND),         &sim->accel_noise[2]);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR1_SCALE_X),     &sim->gyro_scale[0].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR1_SCALE_Y),     &sim->gyro_scale[0].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR1_SCALE_Z),     &sim->gyro_scale[0].z);
#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR2_SCALE_X),     &sim->gyro_scale[1].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR2_SCALE_Y),     &sim->gyro_scale[1].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR2_SCALE_Z),     &sim->gyro_scale[1].z);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR3_SCALE_X),     &sim->gyro_scale[2].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR3_SCALE_Y),     &sim->gyro_scale[2].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR3_SCALE_Z),     &sim->gyro_scale[2].z);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACCEL1_FAIL),      &sim->accel_fail[0]);
#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACCEL2_FAIL),      &sim->accel_fail[1]);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACCEL3_FAIL),      &sim->accel_fail[2]);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_GYR_FAIL_MSK),     &sim->gyro_fail_mask);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC_FAIL_MSK),     &sim->accel_fail_mask);

    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_SCAL_X),      &sim->accel_scale[0].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_SCAL_Y),      &sim->accel_scale[0].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC1_SCAL_Z),      &sim->accel_scale[0].z);

#if INS_MAX_INSTANCES > 1
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_SCAL_X),      &sim->accel_scale[1].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_SCAL_Y),      &sim->accel_scale[1].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC2_SCAL_Z),      &sim->accel_scale[1].z);
#endif
#if INS_MAX_INSTANCES > 2
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_SCAL_X),      &sim->accel_scale[2].x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_SCAL_Y),      &sim->accel_scale[2].y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC3_SCAL_Z),      &sim->accel_scale[2].z);
#endif
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC_TRIM_X),       &sim->accel_trim.x);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC_TRIM_Y),       &sim->accel_trim.y);
    param_link_variable(PARAM_ID(SIM_INS, SIM_ACC_TRIM_Z),       &sim->accel_trim.z);
    param_link_variable(PARAM_ID(SIM_INS, SIM_SAIL_TYPE),        &sim->sail_type);
    param_link_variable(PARAM_ID(SIM_INS, SIM_JSON_MASTER),      &sim->ride_along_master);
    param_link_variable(PARAM_ID(SIM_INS, SIM_OH_MASK),          &sim->on_hardware_output_enable_mask);

#if AP_SIM_SHIP_ENABLED
    sim_ship_ctor(&sim->shipsim);
#endif
}

float sim_measure_distance_at_angle_bf(const Location *location, float angle)
{
    return 0.0f;
}

/* report SITL state via MAVLink SIMSTATE*/
void sim_simstate_send(sim_const_t sim, mavlink_channel_t chan)
{
    float yaw;

    // convert to same conventions as DCM
    yaw = (float)sim->state.yawDeg;
    if (yaw > 180) {
        yaw -= 360;
    }

    mavlink_msg_simstate_send(chan,
                              ToRad((float)sim->state.rollDeg),
                              ToRad((float)sim->state.pitchDeg),
                              ToRad(yaw),
                              (float)sim->state.xAccel,
                              (float)sim->state.yAccel,
                              (float)sim->state.zAccel,
                              radians((float)sim->state.rollRate),
                              radians((float)sim->state.pitchRate),
                              radians((float)sim->state.yawRate),
                              (int32_t)(sim->state.latitude*1.0e7),
                              (int32_t)(sim->state.longitude*1.0e7));
}

/* report SITL state via MAVLink SIM_STATE */
void sim_sim_state_send(sim_const_t sim, mavlink_channel_t chan)
{
    // convert to same conventions as DCM
    float yaw = (float)sim->state.yawDeg;
    if (yaw > 180) {
        yaw -= 360;
    }

    mavlink_msg_sim_state_send(chan,
            sim->state.quaternion.q0,
            sim->state.quaternion.q1,
            sim->state.quaternion.q2,
            sim->state.quaternion.q3,
            ToRad((float)sim->state.rollDeg),
            ToRad((float)sim->state.pitchDeg),
            ToRad(yaw),
            (float)sim->state.xAccel,
            (float)sim->state.yAccel,
            (float)sim->state.zAccel,
            radians((float)sim->state.rollRate),
            radians((float)sim->state.pitchRate),
            radians((float)sim->state.yawRate),
            (float)(sim->state.latitude*1.0e7),
            (float)(sim->state.longitude*1.0e7),
            (float)sim->state.altitude,
            0.0f,
            0.0f,
            (float)sim->state.speedN,
            (float)sim->state.speedE,
            (float)sim->state.speedD);
}

/*
 convert a set of roll rates from earth frame to body frame
 output values are in radians/second
*/
void sim_convert_body_frame(double rollDeg, double pitchDeg,
                              double rollRate, double pitchRate, double yawRate,
                              double *p, double *q, double *r)
{
    double phi, theta, phiDot, thetaDot, psiDot;

    phi = ToRad(rollDeg);
    theta = ToRad(pitchDeg);
    phiDot = ToRad(rollRate);
    thetaDot = ToRad(pitchRate);
    psiDot = ToRad(yawRate);

    *p = phiDot - psiDot*sin(theta);
    *q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
    *r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
}

/*
  convert angular velocities from body frame to
  earth frame.

  all inputs and outputs are in radians/s
*/
Vector3f_t sim_convert_earth_frame(const matrix3f_t *dcm, const Vector3f_t *gyro)
{
    float p = gyro->x;
    float q = gyro->y;
    float r = gyro->z;

    float phi, theta, psi;
    matrix3f_to_euler(dcm, &phi, &theta, &psi);

    float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi));
    float thetaDot = q*cosf(phi) - r*sinf(phi);
    if (fabsf(cosf(theta)) < 1.0e-20f) {
        theta += 1.0e-10f;
    }
    float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta);

    Vector3f_t angular_vel_ef = {phiDot, thetaDot, psiDot};

    return angular_vel_ef;
}

// get the rangefinder reading for the desired rotation, returns -1 for no data
float sim_get_rangefinder(sim_t sim, uint8_t instance)
{
    if (instance < ARRAY_SIZE(sim->state.rangefinder_m)) {
        return sim->state.rangefinder_m[instance];
    }
    return -1.0f;
};

int sim_setup_singleton(void)
{
    sim_ctor(sim_get_singleton());
    return 0;
}
INIT_APP_EXPORT(sim_setup_singleton);
/*------------------------------------test------------------------------------*/


